#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from std_msgs.msg import String
from ball_collector_interfaces.srv import SetGripper
import time

class HardwareTestNode(Node):
    def __init__(self):
        super().__init__('hardware_test_node')
        
        # 创建发布者发送速度命令
        self.cmd_vel_pub = self.create_publisher(
            Twist,
            '/cmd_vel',
            10
        )
        
        # 创建订阅者接收状态信息
        self.status_sub = self.create_subscription(
            String,
            '/hardware_status',
            self.status_callback,
            10
        )
        
        # # 创建夹爪服务客户端
        # self.gripper_client = self.create_client(
        #     SetGripper,
        #     '/hardware_interface/set_gripper'
        # )
    #     self.gripper_client = self.create_client(
    # SetGripper,
    # '/set_gripper'  # 使用正确的服务名称

        self.gripper_client = self.create_client(
            SetGripper,
            '/hardware_interface/set_gripper'
        )
        # 等待服务可用
        while not self.gripper_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('等待夹爪服务可用...')
        
        # 创建定时器定期执行测试
        self.test_timer = self.create_timer(1.0, self.run_test_sequence)
        self.test_step = 0
        
        self.get_logger().info('硬件测试节点已启动')
    
    def status_callback(self, msg):
        self.get_logger().info(f'收到硬件状态: {msg.data}')
    
    def run_test_sequence(self):
        # 测试序列
        if self.test_step == 0:
            self.send_stop_command()
            self.get_logger().info('测试 - 发送停止命令')
        
        elif self.test_step == 1:
            self.send_forward_command()
            self.get_logger().info('测试 - 发送前进命令')
        
        elif self.test_step == 2:
            self.send_rotate_command()
            self.get_logger().info('测试 - 发送旋转命令')
        
        elif self.test_step == 3:
            self.send_stop_command()
            self.get_logger().info('测试 - 发送停止命令')
        
        elif self.test_step == 4:
            self.test_gripper(True)
            self.get_logger().info('测试 - 闭合夹爪')
        
        elif self.test_step == 5:
            self.test_gripper(False)
            self.get_logger().info('测试 - 打开夹爪')
        
        elif self.test_step == 6:
            # 复杂测试：前后移动和转向
            self.get_logger().info('测试 - 开始综合测试序列')
            self.send_forward_command()
            time.sleep(1.0)
            self.send_stop_command()
            time.sleep(0.5)
            self.send_rotate_command()
            time.sleep(1.0)
            self.send_stop_command()
        
        # 循环测试
        self.test_step = (self.test_step + 1) % 7
    
    def send_stop_command(self):
        msg = Twist()
        msg.linear.x = 0.0
        msg.angular.z = 0.0
        self.cmd_vel_pub.publish(msg)
    
    def send_forward_command(self):
        msg = Twist()
        msg.linear.x = 0.5  # 适当的前进速度
        msg.angular.z = 0.0
        self.cmd_vel_pub.publish(msg)
    
    def send_rotate_command(self):
        msg = Twist()
        msg.linear.x = 0.0
        msg.angular.z = 0.5  # 适当的旋转速度
        self.cmd_vel_pub.publish(msg)
    
    def test_gripper(self, grip):
        request = SetGripper.Request()
        request.grip = grip
        request.force = 0.7
        
        future = self.gripper_client.call_async(request)
        future.add_done_callback(self.gripper_callback)
    
    def gripper_callback(self, future):
        try:
            response = future.result()
            self.get_logger().info(f'夹爪控制响应: {response.success}, {response.message}')
        except Exception as e:
            self.get_logger().error(f'调用夹爪服务出错: {str(e)}')

def main(args=None):
    rclpy.init(args=args)
    node = HardwareTestNode()
    
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()